using UnityEngine;
using System.Collections;
using System;
using Decision_Tree;
using DebugTools;

public class AgentBehaviourScript : MonoBehaviour {
	
	private const float fleeRange = 500.0f;
	
	//Agent Physics constants 	
	// Motion Constants
	private const float maxTorque = 4000.0f;
	private const float angleOfMaxTorque = 25.0f;
	
	private const float maxForwardForce = 1000.0f;
	private const float distanceOfMaxForce = 20.0f;
	
	private const float angularDrag = 6.0f;
	private const float drag = 12.0f;
	
	// Collision Constants
	private const float antennaAngle = 160.0f;	//140
	private const int antennaNumOfRays = 5;
	private const float antennaLengthMin = 85.0f; //85
	private const float antennaLengthVelocityFactor = 0.10f;
	private const int antennaSweepDivisions = 8;
	private const float antennaSweepAngle = 200.0f; 
	//End Agent Physics constants 	
	
	private const float eatRange = 120.0f;
	
	private bool finished;
	
	// Path found closest objects updated by plan
	private Plan searchPlan;
	private pathplanner planner;
	private pathplanner searchPlanner;
	
	// Energy level
	private float energyLevel = 0.0f;
	private const float energyDrain = 0.01f;
	private GameObject energyLight; //Light to show how much energy an agent has
	
	// Reference to the decision tree
	private AgentDecisionTree agentDecisionTree;
	
	PhysicsObject physicsObject;

	private ArrayList herders;
	private ArrayList energyPickups;
	//ref to the maze
	private Maze maze;
	
	private Plan currentPlan = null;
	
	public Knowledge brain = new Knowledge();
	
	// Use this for initialization
	public void  Start () 
	{
		energyLight = new GameObject("Agent Energy Light"); 
		energyLight.AddComponent("Light");
		
		this.maze = (((GameController)GameObject. FindWithTag("GameController").GetComponent("GameController")).getMaze());
		
		this.herders = (((GameController)GameObject. FindWithTag("GameController").GetComponent("GameController")).getHerders());
		this.energyPickups = (((GameController)GameObject. FindWithTag("GameController").GetComponent("GameController")).getEnergyPickups());
		gameObject.AddComponent("PhysicsObject");
		physicsObject = (PhysicsObject)gameObject.GetComponent("PhysicsObject");
		physicsObject.init();
		physicsObject.setMass(0.3f);
		physicsObject.setMaxTrust(maxForwardForce);
		physicsObject.SetValues(maxTorque, maxForwardForce, distanceOfMaxForce,
			 angleOfMaxTorque, angularDrag, drag,
			 antennaAngle, antennaNumOfRays, antennaLengthMin,
			 antennaLengthVelocityFactor, antennaSweepDivisions, antennaSweepAngle);
		gameObject.collider.material.dynamicFriction = 0.0f;
		gameObject.collider.material.staticFriction = 0.0f;
		spawn();

	}
	
	public void onDestroy()
	{
		MonoBehaviour.DestroyObject( energyLight );
	}
	
	public void rebuildTree()
	{
		agentDecisionTree.rebuildTree();			
	}
	
	public void spawn()
	{
		if (maze == null)
			return;
		agentDecisionTree = new AgentDecisionTree(this);
		ArrayList spawnArea = maze.getSpawnArea();
		int spawnSquareNum = (int)(UnityEngine.Random.value * (float)spawnArea.Count);	
		Vector3 spawnPoint = maze.getRandomPositionOnSquare( (MazeLocation)(spawnArea[spawnSquareNum]) );
		                                                    
		transform.position = spawnPoint;	
		setDestination(getPosition()); //Set not to move
		transform.rotation = new Quaternion();	// I think will reset rotation
		
		finished = false;
		
		energyLevel = 100.0f;
		
		energyLight.transform.position = transform.position;
		energyLight.light.color = Color.blue;
		energyLight.light.range = 100.0f;
		energyLight.light.intensity = 1.8f;	
		
		searchPlan = new PlanUpdateClosest(this, maze, herders, energyPickups, getNearestExit());
		
	}
	
	public void removeEnergy(float amount)
	{
		energyLevel -= amount;	
		if (energyLevel	< 0.0f) energyLevel = 0.0f;
	}
	
	// Called every frame
	public void Update ()
	{

		energyLight.transform.position = rigidbody.transform.position;
		energyLight.light.range = energyLevel * energyLevel *0.005f;
		energyLight.light.color = new Color(0.0f, 0.0f, energyLevel/100.0f, 1.0f);
		
		// draws paths, open list and closed list
		// planner.debug_drawing();
		Vector3 agentPos = this.getPosition();
		
		/*if (getNearestHerder())
			Debug.DrawLine(getPosition(), getNearestHerder().getPosition(), Color.red);
		if (getNearestEnergy())
			Debug.DrawLine(getPosition(), getNearestEnergy().getPosition(), Color.yellow);
			*/
			
		float distToGoal = getPathDistanceToGoal();
		
		float distToFood = 0;
		if(this.getNearestEnergy() != null)
			distToFood = Vector3.Distance(agentPos, this.getNearestEnergy().getPosition());
		
		float distToHerder = (float)this.getDistanceToNearestHerder();
		float timeLeft = (((GameController)GameObject. FindWithTag("GameController").GetComponent("GameController")).getRemaingRoundTime());
		float food = this.getEnergy();
		
		
		this.brain.setKnowledge(distToGoal,distToFood,distToHerder,timeLeft,food);
		
		/*
		// Draw the static values for attributes as circles
		DebugTool.drawCircle(this.getPosition(),
			                     (float)Decision_Tree.AT.THRESH_B_CLOSE_TO_FOOD, 
			                     new Color(1.0f, 0.0f, 0.0f));
		
		// Draw the static values for attributes as circles
		DebugTool.drawCircle(this.getPosition(),
			                     (float)Decision_Tree.AT.THRESH_B_CLOSE_TO_GOAL, 
			                     new Color(0.0f, 1.0f, 0.0f));
		
		// Draw the static values for attributes as circles
		DebugTool.drawCircle(this.getPosition(),
			                     (float)Decision_Tree.AT.THRESH_B_CLOSE_TO_HERDER, 
			                     new Color(0.0f, 0.0f, 1.0f));
		*/
		
	}
	

	// This function is called once every physics time step.
	public void FixedUpdate () 
	{
		EnergyPickup nearestEnergy= getNearestEnergy();
		if (nearestEnergy)
		{
		
			if ( Vector3.Distance( nearestEnergy.getPosition(), getPosition() ) < eatRange )
			{
				energyLevel += nearestEnergy.getEnergyLevel();	
				if (energyLevel > 300.0f) energyLevel = 300.0f;
				energyPickups.Remove(nearestEnergy);
				Destroy(nearestEnergy.gameObject);
			}	
		}
		
		energyLevel -= Time.deltaTime * energyDrain * rigidbody.velocity.magnitude;
		if (energyLevel < 0.0f) energyLevel = 0.0f;
		physicsObject.setMaxTrust((energyLevel/100.0f)*maxForwardForce);
		
		// A* planning
		
		//Don't make any plans if we are on an exit	
		
		if (!finished)
		{
			finished = maze.getMazeSquare(maze.getMazeCoord(getPosition())).isFinish();
		}
		else
		{
			currentPlan = null;
			searchPlan = null;
			setDestination(getPosition());
		}	
		
		if (!finished)
		{
			currentPlan = agentDecisionTree.getCurrentPlan();
			if (searchPlan != null)
				searchPlan.execute();
			if (currentPlan	!= null)
			{
				currentPlan.execute();	
				
				//planner = currentPlan.getPathPlanner();
				//if (planner != null) planner.debug_drawing();	
				
			}
			if (searchPlan.getPathPlanner() != null)
				searchPlanner = searchPlan.getPathPlanner();
			if (searchPlanner != null) searchPlanner.debug_drawing();	
			
		}
		
		physicsObject.updatePhysics();
		
	}
	
	public Plan getCurrentPlan()
	{
		return currentPlan;	
	}
	
	public Plan getSearchPlan()
	{
		return searchPlan;	
	}
		
	public AgentDecisionTree getAgentDecisionTree()
	{
		return agentDecisionTree;	
	}
	
	public float getEnergy()
	{
		return energyLevel; 	
	}
	
	public float getPathDistanceToGoal()
	{
		if (searchPlan != null)
			return ((PlanUpdateClosest)searchPlan).getExitDistance();
		else
			return -1.0f;	
	}

	// Finds the nearest exit to a position
	public Vector3 getNearestExit()
	{
		Vector3 position = getPosition();
		float shortestDist = -1.0f;
		Vector3 shortestPos = position;
		
		foreach ( Vector3 finish in maze.getExits() )
		{
			if (Vector3.Distance(finish, position) < shortestDist || shortestDist < 0)
			{
				shortestDist = Vector3.Distance(finish, position);
				shortestPos = finish;
			}
		}
		return shortestPos;
	}	
	
	public EnergyPickup getNearestEnergy()
	{
		if (searchPlan != null)
			return ((PlanUpdateClosest)searchPlan).getClosestEnergy();
		else
			return null;
	}
	
	/*
	public Vector3 getNearestHerderPosition()
	{
		/*
		// Hack to make agents scared of more than one herder - Russell	
		Vector3 sumPosition = new Vector3(0,0,0);
		
		float smallestDist = 100000.0f;
		int count = 0;
        foreach (GameObject gameObject in herders) 
		{
			HerderBehaviourScript herder = (HerderBehaviourScript)gameObject.GetComponent("HerderBehaviourScript");	
			if (Vector3.Distance(herder.getPosition(), getPosition()) < getFleeRange())
			{
				if (Vector3.Distance(herder.getPosition(), getPosition()) < smallestDist) 
					smallestDist = Vector3.Distance(herder.getPosition(), getPosition());
				
				sumPosition += (herder.getPosition() - getPosition()).normalized;	
				count++;
			}
		}
			
		// This just pretends there is only one herder at average direction-ish of the agents within flee range
		// as close as the closest herder
		sumPosition /= count;
		
		Vector3 res = sumPosition * smallestDist;
		return res;
		
		
		return getNearestHerder().getPosition();
	}*/
	
	public float getDistanceToNearestHerder()
	{
		if (getNearestHerder() == null)
			return -1.0f; // I don't know where the nearest herder is yet
		if (searchPlan != null)
			return ((PlanUpdateClosest)searchPlan).getClosestHerderDistance();
		else
			return 0.0f;
	}
	
	public float getDistanceToNearestEnergy()
	{
		if (getNearestEnergy() == null)
			return -1.0f; // I don't know where the nearest herder is yet
			
		if (searchPlan != null)
			return ((PlanUpdateClosest)searchPlan).getClosestEnergyDistance();
		else
			return 0.0f;
	}
	
	public HerderBehaviourScript getNearestHerder()
	{
		if (searchPlan != null)
			return ((PlanUpdateClosest) searchPlan).getClosestHerder();
		else
			return null;
	}
		
	
	public void ChangeColour (Color targetColour)
	{
		renderer.material.color = targetColour;
	}


	// Get the Fleeing range
	private float getFleeRange()
	{
		return fleeRange;
	}
	
	public bool isInFleeRange()
	{
		// return false if there are not any herders
		if (herders.Count == 0)
			return false;
		
		return (getDistanceToNearestHerder() < getFleeRange());
	}
	
	public Vector3 averagePositionOfNeighbours()
	{
		float max_distance_nearest_neighbour = 200.0f;
		ArrayList agents = (((GameController)GameObject.FindWithTag("GameController").GetComponent("GameController")).getAgents());	
		ArrayList neighbours = new ArrayList();
		
		foreach (GameObject agent in agents) 
		{
			Vector3 agent_position = ((AgentBehaviourScript)agent.GetComponent("AgentBehaviourScript")).getPosition();
			float dist = Vector3.Distance(agent_position, getPosition());
			
			if(dist > 0 && dist < max_distance_nearest_neighbour)
			{			
				neighbours.Add(((AgentBehaviourScript)agent.GetComponent("AgentBehaviourScript")));
			}
        }
		
		if(neighbours.Count == 0)
			return new Vector3(0,0,0);
		
		Vector3 result = new Vector3();
		
		foreach (AgentBehaviourScript agent in neighbours)	
			result += agent.getPosition();
		return result/neighbours.Count;
	}
	
	public PhysicsObject getPhysicsObject()
	{
		return physicsObject;	
	}
		
	
	public Vector3 getPosition()
	{
		return transform.position;
	}
	
	public float getEatRange()
	{
		return eatRange;	
	}
	
	public void setDestination(Vector3 position)
	{
		physicsObject.setDestination(position);
	}
	
	public Vector3 getDestination()
	{
		return physicsObject.getDestination();
	}

}
